% yysh
% read pointcloud data from .pcd file
% pointcloud data is stored in data field
function PCD = pcdread(filename)
  
  fid = fopen(filename);
  
  fgetl(fid); % #
  
  h=struct();
  h = parse_line(h, fgetl(fid)); %VERSION
  h = parse_line(h, fgetl(fid)); %FIELDS
  h = parse_line(h, fgetl(fid)); %SIZE
  h = parse_line(h, fgetl(fid)); %TYPE
  h = parse_line(h, fgetl(fid)); %COUNT
  h = parse_line(h, fgetl(fid)); %WIDTH
  h = parse_line(h, fgetl(fid)); %HEIGHT
  h = parse_line(h, fgetl(fid)); %VIEWPOINT
  h = parse_line(h, fgetl(fid)); %POINTS
  h = parse_line(h, fgetl(fid)); %DATA
  
  PCD.fields = h.FIELDS;
  PCD.width = str2num(h.WIDTH{1});
  PCD.height =str2num(h.HEIGHT{1});
  for i = 1:size(h.VIEWPOINT)(2)
    PCD.viewpoint(i) = str2num(h.VIEWPOINT{i});
  endfor
  PCD.points = str2num(h.POINTS{1});

  fmt = parse_fmt(h);
  
  if strcmp(h.DATA{1}, 'ascii')
    PCD.data = dlmread (filename, ' ', 11, 0);
  elseif strcmp(h.DATA{1}, "binary")
    for i=1:PCD.points
      n = 1;
      for j=1:length(h.FIELDS)
          PCD.data(i,n:n+fmt{j,1}-1) = fread(fid, fmt{j,1}, fmt{j,2});
          n=n+fmt{j,1};
      endfor
    endfor
  else
    error ("bad pcd data");
    fclose(fid);
    return
  endif
 
  fclose(fid);
endfunction

function fmt = parse_fmt(h)
  fmt = cell(length(h.SIZE),2);
  for i=1:length(h.SIZE)
    fmt{i, 1} = str2num(h.COUNT{i});
    if h.TYPE{i} == 'F'
      fmt{i, 2} = "float32";
    elseif h.TYPE{i} == 'U'
      if h.SIZE{i} == '1'
        fmt{i, 2} = "uint8";
      elseif h.SIZE{i} == '2'
        fmt{i, 2} = "uint16";
      elseif h.SIZE{i} == '4'
        fmt{i, 2} = "uint32 ";
      endif
    elseif h.TYPE{i} == 'I'
      if h.SIZE{i} == '1'
        fmt{i, 2} = "int8";
      elseif h.SIZE{i} == '2'
        fmt{i, 2} = "int16";
      elseif h.SIZE{i} == '4'
        fmt{i, 2} = "int32";
      endif
    endif
  endfor
endfunction

function f = parse_line(o, str)
  [t,r] = strtok(str);
  o.(strsplit(t){1}) = strsplit(strtrim(r));
  f = o;
endfunction
